/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief This is a simple example for using the Ranging interface.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "RangingClient.hpp"

//*****************************************************************************
#define GIMI_CLIENT_API 20000
#include "gimi.h"
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  ///// Check parameters.
  // We need a few parameters in order to run the example. Check that
  // they were specified.
  if (argc != 4) {
    printf("\nUsage: %s <GIMnetAP host> <GIMnetAP port> <MaCI Service Locator>\n", 
           argv[0]);
    printf("For example: '%s asrobo.hut.fi 40002 J2B2.MaCI_Ranging.Laser'\n\n", argv[0]);
    exit(1);
  }

  ////////////////////////////////////////////////////////////////////////////  
  ///// Initialize required components for establishing a connection
  ///// to an existing MaCI Ranging service.

  // Construct GIMI instance (Communication layer - no parameters yet)
  gimi::GIMI g;

  // Construct a RangingClient instance, and assign the GIMI instance
  // to it (For communication)
  MaCI::Ranging::CRangingClient rc(&g);

  // Connect to GIMnetAP. If the operation fails - exit!
  if (g.connectToHubEx(argv[1], atoi(argv[2]), "") != GIMI_OK) {
    printf("Failed to connect to GIMnetAP at %s:%d!\n",
           argv[1], atoi(argv[2]));
    exit(1);
  }


  ///// If we got so far, we have successfully established a GIMnet
  ///// connection with given parameters. Next, Assign a target service
  ///// for the RangingClient, using the third provided parameter.
  
  // The expression given as third argument should be in MaCISL (MaCI
  // Service Locator)
  // format. (<Group>.<InterfaceName>.<InstanceName>). Now, construct
  // a MaCISL object from the provided string.
  const MaCI::MaCICtrl::SMaCISL sl(argv[3]);
  
  // Next, assign this target to the RangingClient.
  rc.SetDefaultTarget(sl);
  
  // Next, attempt to open the interface. Opening the interface
  // subscribes to the datafeed provided by the service.
  if (rc.Open() != MaCI::KMaCIOK) {
    printf("Failed to open the request Ranging service '%s'!\n",
           argv[3]);
    exit(1);
  }

  
  ///// Initialization have been completed successfully. Now its time
  ///// to start receiving and processing the data.
  // Initialize required variables, index, timestamp, header and the
  // array itself.
  int index = -1;
  MaCI::Common::TTimestamp timestamp;
  MaCI::Ranging::TDistanceHeader header;
  MaCI::Ranging::TDistanceArray array;

  // Now, receive 10 measurements.
  int count = 10;
  while(count--) {

    // Get an array of measurements from the RangingClient.
    printf("Requesting array...");
    const bool gr = rc.GetDistanceArray(array, &header, &timestamp, &index, 1000);
    if (gr) {

      // We got 'true', so we got an array of data before
      // timeout. Print some info.
      printf("Got array... (Contains %d Distances)\n", header.beam_count);
      
      // Iterate over the result array. Every value is in 'i' per
      // turn. (members distance and angle) This example just prints
      // the values formatted to 6 columns.
      int offset = 0;
      for(EACH_IN_i(array)) {
        printf("%5.2fm @ %5.2frad, ", i->distance, i->angle); 
        if (offset++ == 5) { printf("\n"); offset=0; }
      }
    }
  }
}


